#include <iostream>
#include <stdlib.h>
#include <math.h>

#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/legacy/legacy.hpp"
#include "opencv2/legacy/compat.hpp"


using namespace std;
#ifdef _DEBUG
#pragma comment(lib, "..//opencv_lib//opencv_core240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_imgproc240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_highgui240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_flann240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_calib3d240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_contrib240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_features2d240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_legacy240d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_nonfree240d.lib")
#else
#pragma comment(lib, "..//opencv_lib//opencv_core240.lib")
#pragma comment(lib, "..//opencv_lib//opencv_imgproc240.lib")
#endif
void cvmSet6(CvMat* m, int row, int col, float val1, float val2, float val3, float val4, float val5, float val6)
{
	cvmSet(m, row, col, val1);
	cvmSet(m, row, col + 1, val2);
	cvmSet(m, row, col + 2, val3);
	cvmSet(m, row, col + 3, val4);
	cvmSet(m, row, col + 4, val5);
	cvmSet(m, row, col + 5, val6);
}

void FindAffineTransform(const vector<CvPoint2D32f>& p1, const vector<CvPoint2D32f>& p2, CvMat* affine)
{
	int eq_num = 2*(int)p1.size();
	CvMat* A = cvCreateMat(eq_num, 6, CV_32FC1);
	CvMat* B = cvCreateMat(eq_num, 1, CV_32FC1);
	CvMat* X = cvCreateMat(6, 1, CV_32FC1);

	for(int i = 0; i < (int)p1.size(); i++)
	{
		cvmSet6(A, 2*i, 0, p1[i].x, p1[i].y, 1, 0, 0, 0);
		cvmSet6(A, 2*i + 1, 0, 0, 0, 0, p1[i].x, p1[i].y, 1);
		cvmSet(B, 2*i, 0, p2[i].x);
		cvmSet(B, 2*i + 1, 0, p2[i].y);
	}

	cvSolve(A, B, X, CV_SVD);

	cvmSet(affine, 0, 0, cvmGet(X, 0, 0));
	cvmSet(affine, 0, 1, cvmGet(X, 1, 0));
	cvmSet(affine, 0, 2, cvmGet(X, 2, 0));
	cvmSet(affine, 1, 0, cvmGet(X, 3, 0));
	cvmSet(affine, 1, 1, cvmGet(X, 4, 0));
	cvmSet(affine, 1, 2, cvmGet(X, 5, 0));

	cvReleaseMat(&A);
	cvReleaseMat(&B);
	cvReleaseMat(&X);
}

double
compareSURFDescriptors( const CvPoint2D32f* d1, const CvPoint2D32f* d2, double best, int length )
{
	double total_cost = 0;

	for( int i = 0; i < 2; i += 4 )
	{
		double t0 = d1[i  ].x - d2[i  ].x;
		double t1 = d1[i].y - d2[i+1].y;

		total_cost += t0*t0 + t1*t1;// + t2*t2 + t3*t3;
		if( total_cost > best )
			break;
	}
	return total_cost;
}


int
naiveNearestNeighbor( const CvPoint2D32f* vec,const CvPoint2D32f*d2, const int total )					 
					
{
	int length = 2;
	int i, neighbor = -1;
	double d, dist1 = 1e6, dist2 = 1e6;
	CvSeqReader reader, kreader;

    const CvPoint2D32f*mvec =d2;


	for( i = 0; i < total; i++ )
	{



		d = compareSURFDescriptors( vec, mvec, dist2, length );
		if( d < dist1 )
		{
			dist2 = dist1;
			dist1 = d;
			neighbor = i;
		}
		else if ( d < dist2 )
			dist2 = d;
	}
	//if ( dist1 < 0.2*dist2 )
		return neighbor;
	//return -1;
	
	mvec++;

  }


void
findPairs( const CvPoint2D32f* objectKeypoints, const CvPoint2D32f* imageKeypoints,
		  const int objecttale, const int imagetale, vector<int>& ptpairs )
{
	int i;

	ptpairs.clear();

	for( i = 0; i < objecttale; i++ )
	{

		int nearest_neighbor = naiveNearestNeighbor( &objectKeypoints[i],  &imageKeypoints[i],imagetale );
		if( nearest_neighbor >= 0 )
		{
			ptpairs.push_back(i);
			ptpairs.push_back(nearest_neighbor);
		}
	}
}

//by Huang, Haiqiao 5 Dec. 2009
int main(int argc, char** argv)
{
	cout << "Corner Detection OpenCV!"<<endl; 
	char* filename="5.bmp";
	char* filename2="4.bmp";
	IplImage* imgRGB = cvLoadImage(filename); 
	IplImage* imgRGB2 = cvLoadImage(filename); 
	IplImage* imgGrey = cvLoadImage(filename,CV_LOAD_IMAGE_GRAYSCALE); 

	IplImage* object = cvLoadImage( filename, CV_LOAD_IMAGE_GRAYSCALE );
	IplImage* image = cvLoadImage( filename2, CV_LOAD_IMAGE_GRAYSCALE );

	IplImage* sub_obiect = cvCreateImage(cvGetSize(object), 8, 1);

	if (imgGrey==NULL){//image validation
		cout << "No valid image input."<<endl; 
		char c=getchar();
		return 1;
	} 
	int w=imgGrey->width;
	int h=imgGrey->height;

	IplImage* eig_image = cvCreateImage(cvSize(w, h),IPL_DEPTH_32F, 1);
	IplImage* temp_image = cvCreateImage(cvSize(w, h),IPL_DEPTH_32F, 1); 

	const int MAX_CORNERS = 140;//estimate a corner number
	CvPoint2D32f corners[MAX_CORNERS] = {0};// coordinates of corners
	CvPoint2D32f corners2[MAX_CORNERS] = {0};// coordinates of corners
	//CvPoint2D32f* corners = new CvPoint2D32f[ MAX_CORNERS ]; //another method of declaring an array
	int corner_count = MAX_CORNERS; 
	int corner_count2 = MAX_CORNERS; 
	double quality_level = 0.01;//threshold for the eigenvalues
	double min_distance = 50;//minimum distance between two corners
	int eig_block_size = 6;//window size
	int use_harris = false;//use 'harris method' or not

	//----------initial guess by cvGoodFeaturesToTrack---------------
	cvGoodFeaturesToTrack(imgGrey,
		eig_image,   // output                 
		temp_image,
		corners,
		&corner_count,
		quality_level,
		min_distance,
		NULL,
		eig_block_size,
		use_harris);


	int r=2; //rectangle size
	int lineWidth=1; // rectangle line width
	//-----draw good feature corners on the original RGB image---------
	for (int i=0;i<corner_count;i++){
		cvRectangle(imgRGB2, cvPoint(corners[i].x-r,corners[i].y-r), 
			cvPoint(corners[i].x+r,corners[i].y+r), cvScalar(255,0,0),lineWidth);
	}

	cvNamedWindow("cvGoodFeaturesToTrack", CV_WINDOW_AUTOSIZE );
	cvShowImage( "cvGoodFeaturesToTrack", imgRGB2 );

	int half_win_size=3;//the window size will be 3+1+3=7
	int iteration=20;
	double epislon=0.1; 
	cvFindCornerSubPix(
		imgGrey,
		corners,
		corner_count,
		cvSize(half_win_size,half_win_size),
		cvSize(-1,-1),//no ignoring the neighbours of the center corner
		cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,iteration,epislon)
		);


///file2//////////////////////////////////////////////////
	imgRGB = cvLoadImage(filename2); 
	imgRGB2 = cvLoadImage(filename2); 
	imgGrey = cvLoadImage(filename2,CV_LOAD_IMAGE_GRAYSCALE); 


	cvGoodFeaturesToTrack(imgGrey,
		eig_image,   // output                 
		temp_image,
		corners2,
		&corner_count2,
		quality_level,
		min_distance,
		NULL,
		eig_block_size,
		use_harris);


	cvFindCornerSubPix(
		imgGrey,
		corners2,
		corner_count2,
		cvSize(half_win_size,half_win_size),
		cvSize(-1,-1),//no ignoring the neighbours of the center corner
		cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,iteration,epislon)
		);

/////////////////////////////////////////////////////////////

	vector<int> ptpairs;

	vector<CvPoint2D32f> objectpoint;
	vector<CvPoint2D32f> featurepoint;

	findPairs(corners, corners2,corner_count, corner_count2, ptpairs );



   int i;
	//for(i=0;i<ptpairs.size();i+=2)
	{
		CvPoint2D32f F1,F2;


		

          F1 = corners[6];
		   F2 = corners2[5];

		objectpoint.push_back(F1);
		featurepoint.push_back(F2);

		F1 = corners[7];
		F2 = corners2[6];

		objectpoint.push_back(F1);
		featurepoint.push_back(F2);

		F1 = corners[8];
		F2 = corners2[8];

		objectpoint.push_back(F1);
		featurepoint.push_back(F2);

		F1 = corners[9];
		F2 = corners2[7];

		objectpoint.push_back(F1);
		featurepoint.push_back(F2);

		F1 = corners[11];
		F2 = corners2[10];

		objectpoint.push_back(F1);
		featurepoint.push_back(F2);
	 
	}
		

//////////////////////////transform  cvCornerHarris

	float m[6];
	CvMat M = cvMat (2, 3, CV_32F, m);
	////CvMat _h = cvMat(3, 3, CV_64F, h);

	//vector<CvPoint2D32f> objectpoint;
	//vector<CvPoint2D32f> featurepoint;
 //   int i;
 //  for(i=0;i<9;i++)
 //  {
	//objectpoint.push_back(corners[i]);
	//featurepoint.push_back(corners2[i]);
 //  }

	FindAffineTransform(objectpoint,featurepoint,&M);

	////cvgoodfeaturestotrack


	cvWarpAffine( object, sub_obiect, &M);

	cvAbsDiff(sub_obiect,image,object);
 //   int nImatches =36;//min(corner_count,corner_count2);

	//CvMat *points1 =cvCreateMat(2,nImatches,CV_32F);
	//CvMat *points2 =cvCreateMat(2,nImatches,CV_32F);
	//CvMat *fundMatr=cvCreateMat(3,3,CV_32F);
	//CvMat *status  =cvCreateMat(1,nImatches,CV_32F);
	////CvMat *corrLines1=cvCreateMat(3,nImatches,CV_32F);
	////CvMat *corrLines2=cvCreateMat(3,nImatches,CV_32F);


	//for(int i=0;i<nImatches;i++)
	//{
	//	cvmSet(points1,0,i,corners[i].x); cvmSet(points1,1,i,corners[i].y);
	//	cvmSet(points2,0,i,corners2[i].x); cvmSet(points2,1,i,corners2[i].y);
	//}

 //  int num=cvFindFundamentalMat(points1,points2,fundMatr,CV_FM_RANSAC,1.0,0.99,status);	


 //   cvWarpPerspective( object, sub_obiect, fundMatr );

	//------draw subpix corners on another original RGB image------------
	for (int i=0;i<corner_count2;i++){
		cvRectangle(imgRGB, cvPoint(corners2[i].x-r,corners2[i].y-r), 
			cvPoint(corners2[i].x+r,corners2[i].y+r), cvScalar(0,255,255),lineWidth);
	}

	//to display a coordinate of the third corner
	cout<<"x="<<corners[2].x;
	cout<<",y="<<corners[2].y<<endl;

	cvNamedWindow("cvFindCornerSubPix", CV_WINDOW_AUTOSIZE );
	cvShowImage( "cvFindCornerSubPix", object );
	//cvNamedWindow("cvGoodFeaturesToTrack", CV_WINDOW_AUTOSIZE );
	//cvShowImage( "cvGoodFeaturesToTrack", imgRGB2 );


	cvWaitKey(0); 
	cvReleaseImage(&imgGrey);
	cvReleaseImage(&imgRGB);
	cvReleaseImage(&imgRGB2);
	cvDestroyWindow("cvGoodFeaturesToTrack");
	cvDestroyWindow("cvFindCornerSubPix");

	//char c=getchar();
	return 0;
}

